#!/usr/bin/python3

import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped, Point
import time
import numpy as np
from std_msgs.msg import Float64MultiArray, Header, Bool
from sensor_msgs.msg import LaserScan, JointState
from gazebo_msgs.msg import ModelStates
from visualization_msgs.msg import Marker, MarkerArray
import copy
from tf2_geometry_msgs import tf2_geometry_msgs
import sys
from laser_dizuo.srv import MeasureSinglePoint, MeasureSinglePointResponse
from laser_dizuo.msg import NamedPoints
import re
import threading

class measuring_system_service():
    def __init__(self,nodename, ns_prefix):
        rospy.init_node(nodename)
        debug_prefix = "/" + ns_prefix + "/"
        # debug_prefix = ""

        self.debug_prefix = debug_prefix
        self.lock = threading.Lock()
        #parameter initialization for ros_control
        self.laser_num = rospy.get_param(debug_prefix + "laser_group_config/single_laser_num")

        #service and topic initialization for measuring system
        self.sub_point_target = rospy.Subscriber(debug_prefix + "point_target", PointStamped, self.point_target_cb)
        self.sub_update_result = rospy.Subscriber(debug_prefix + "update_measure_result", NamedPoints, self.update_measure_cb)
        self.meas_point_srv = rospy.Service(debug_prefix + "measure_single_point", MeasureSinglePoint, self.cb_measure_single_point)

        self.laser_scan = []
        self.laser_result = []
        for i in range(self.laser_num):
            self.laser_scan.append(rospy.Subscriber(debug_prefix + "laser"+ str(i) + "_laser/scan", LaserScan, self.cb_laser_scan, queue_size=10))
            self.laser_result.append([0,0,0])

        self.pub = rospy.Publisher(debug_prefix + 'laser_joint_states', JointState, queue_size=1)
        self.meas_pub = rospy.Publisher(debug_prefix + 'measuring_result_display', MarkerArray, queue_size=1)
        self.laser_line_pub = rospy.Publisher(debug_prefix + 'laser_line_display', Marker, queue_size=1)

        self.js = JointState()
        self.js.header = Header()
        self.js.name = []
        self.js.position = []
        self.js.velocity = []
        self.js.effort = []

        self.real_joint_states = rospy.Subscriber(debug_prefix + "cmd_joint_states",JointState,self.realJointStates_cb)
        self.turn_on_realtime_tracking = rospy.Subscriber(debug_prefix + "turn_on_realtime_tracking",Bool, self.turnOnRealtimeTracking_cb)
        self.sim_ptp_controller_command = rospy.Publisher(debug_prefix + 'ptp_controller/command', Float64MultiArray, queue_size=10)

        self.joint_name_index = None
        self.realtime_tracking_switch_on = True

        #tf related initialization
        self.laser_base = [ns_prefix + "/laser"+str(i)+"_point_link" for i in range(self.laser_num)]
        self.laser_joints = [["laser"+str(i)+"_joint1", "laser"+str(i)+"_joint2"] for i in range(self.laser_num)]
        self.base = ns_prefix + "/base_link"
        self.point_link_prefix = ns_prefix + "/laser"

        self.tfBuffer = tf2_ros.Buffer()
        self.tfListener = tf2_ros.TransformListener(self.tfBuffer)

    def realJointStates_cb(self,msg):
        if(not self.realtime_tracking_switch_on):
            return 

        if(self.joint_name_index is None):
            para_name = self.debug_prefix + "ptp_controller/joints"
            if(rospy.has_param(para_name)):
                self.joint_names = rospy.get_param(para_name)
                self.joint_name_index = {self.joint_names[i]:i for i in range(len(self.joint_names))}
            else:
                return

        sent_command=[0.0]*len(self.joint_names)
        for i in range(len(msg.name)):
            sent_command[self.joint_name_index[msg.name[i]]] = msg.position[i]
        
        self.sim_ptp_controller_command.publish(Float64MultiArray(data=sent_command))
        return

    def turnOnRealtimeTracking_cb(self,msg):
        print(msg)
        return

    def cb_laser_scan(self,msg):
        frame_id = msg.header.frame_id
        paths = frame_id.split("/")

        pattern = r'\d+'
        laser_num = int(re.findall(pattern, paths[-1])[0])
        self.lock.acquire()
        self.laser_result[laser_num][0] = msg.ranges[0]
        self.laser_result[laser_num][1] = msg.ranges[1]
        self.laser_result[laser_num][2] = msg.ranges[2]
        # print("laser" + str(laser_num) + ":", self.laser_result[laser_num])

        marker = Marker()
        # marker.header.frame_id = self.point_link_prefix + str(laser_num) + "_point_link"
        marker.header.frame_id = self.base
        marker.header.stamp = rospy.Time.now()
        marker.ns = "laser_scan"
        marker.type = Marker.LINE_LIST
        marker.action = Marker.MODIFY
        marker.pose.orientation.w = 1.0
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        marker.color.a = 1.0
        marker.scale.x = 0.01
        marker.scale.y = 0.01 
        marker.scale.z = 1.0
        marker.lifetime = rospy.Duration()

        for i in range(len(self.laser_result)):
            marker.id = i

            pt = PointStamped()
            pt.header.frame_id = self.point_link_prefix + str(i) + "_link2"
            pt.header.stamp=rospy.Time(0,0)
            pt.point.x = self.laser_result[i][1] if self.laser_result[i][1]<30.0 else 30.0
            pt.point.y = 0.0
            pt.point.z = 0.0
        
            if(not self.tfBuffer.can_transform(self.base,pt.header.frame_id,rospy.Time(0,0))):
                continue
            ept = self.tfBuffer.transform(pt, self.base)

            pt.point.x = 0.1
            pt.point.y = 0.0
            pt.point.z = 0.0
            spt = self.tfBuffer.transform(pt, self.base)
            marker.points.append(spt.point)
            marker.points.append(ept.point)

        self.laser_line_pub.publish(marker)
        self.lock.release()

    def cb_measure_single_point(self,req):
        ret = req.point.header.frame_id.split(":")

        if (len(ret)!=2):
            return
        laser_id = int(ret[1])
        if(laser_id not in range(self.laser_num)):
            return

        header = Header()
        header.frame_id = ret[0]
        
        src = tf2_geometry_msgs.PointStamped(header, req.point.point)
        src.point.x = src.point.x / 1000.0
        src.point.y = src.point.y / 1000.0
        src.point.z = src.point.z / 1000.0

        tgt = self.tfBuffer.transform(src, self.laser_base[laser_id], rospy.Duration(1.0))
        exp_range = np.linalg.norm(np.array([tgt.point.x,tgt.point.y,tgt.point.z])) 
        measured_range = self.laser_result[laser_id][1]

        return MeasureSinglePointResponse(exp_result=exp_range,measured_result = measured_range)

    def update_measure_cb(self,msg):
        point_num = len(msg.names)
        if(point_num != len(msg.points)):
            return

        ret = msg.header.frame_id.split(":")
        if (len(ret)!=2):
            return
        namespace = ret[1]
        frame_id = ret[0]
        
        point_map = {msg.names[i]:msg.points[i] for i in range(len(msg.names))}

        markerArray = MarkerArray()
        marker = Marker()

        markers = []
        for i in range(point_num):
            marker.header.frame_id = frame_id
            marker.header.stamp = rospy.Time.now()
            marker.ns = namespace
            marker.id = i
            marker.type = Marker.TEXT_VIEW_FACING
            # marker.text = '前视面板'
            marker.text = msg.names[i] + "[x:" + "{:.1f}".format(msg.points[i].x) + ";]"
            marker.action = Marker.ADD
            marker.pose.position.x = msg.points[i].x / 1000.0
            marker.pose.position.y = msg.points[i].y / 1000.0
            marker.pose.position.z = msg.points[i].z / 1000.0 + 0.1
            marker.pose.orientation.x = 0.0
            marker.pose.orientation.y = 0.0
            marker.pose.orientation.z = 0.0
            marker.pose.orientation.w = 1.0
            marker.color.r = 0.0
            marker.color.g = 1.0
            marker.color.b = 0.0
            marker.color.a = 1.0
            marker.scale.x = 0.1
            marker.scale.y = 0.1
            marker.scale.z = 0.1
            marker.lifetime = rospy.Duration()
            marker.frame_locked=True

            curr = copy.deepcopy(marker)
            markers.append(curr)

        for i in range(point_num):
            marker.header.frame_id = frame_id
            marker.header.stamp = rospy.Time.now()
            marker.ns = namespace
            marker.id = point_num+i
            marker.type = Marker.SPHERE
            # marker.text = '前视面板'
            marker.action = Marker.ADD
            marker.pose.position.x = msg.points[i].x / 1000.0
            marker.pose.position.y = msg.points[i].y / 1000.0
            marker.pose.position.z = msg.points[i].z / 1000.0
            marker.pose.orientation.x = 0.0
            marker.pose.orientation.y = 0.0
            marker.pose.orientation.z = 0.0
            marker.pose.orientation.w = 1.0
            marker.color.r = 1.0
            marker.color.g = 0.0
            marker.color.b = 0.0
            marker.color.a = 1.0
            marker.scale.x = 0.1
            marker.scale.y = 0.1
            marker.scale.z = 0.1
            marker.lifetime = rospy.Duration()
            marker.frame_locked=True

            curr = copy.deepcopy(marker)
            markers.append(curr)

        markerArray.markers = markers
        self.meas_pub.publish(markerArray)

    def point_target_cb(self,msg):
        ret = msg.header.frame_id.split(":")

        if (len(ret)!=2):
            return
        laser_id = int(ret[1])
        if(laser_id not in range(self.laser_num)):
            return

        header = Header()
        header.frame_id = ret[0]
        
        src = tf2_geometry_msgs.PointStamped(header, msg.point)
        src.point.x = src.point.x / 1000.0
        src.point.y = src.point.y / 1000.0
        src.point.z = src.point.z / 1000.0

        tgt = self.tfBuffer.transform(src, self.laser_base[laser_id], rospy.Duration(1.0))
        x = np.linalg.norm(np.array([tgt.point.x,tgt.point.y,tgt.point.z])) 

        theta = 0.0
        alpha = 0.0

        if(x != 0.0):
            theta = math.atan2(tgt.point.y, tgt.point.x)
            if(math.fabs(math.cos(theta))<0.2):
                alpha = math.atan2(-tgt.point.z, tgt.point.y/math.sin(theta))
            else:
                alpha = math.atan2(-tgt.point.z, tgt.point.x/math.cos(theta))

        self.js.name = self.laser_joints[laser_id] 
        self.js.position = [theta,alpha]
        self.pub.publish(self.js)

    def run(self):
        '''
        rate = rospy.Rate(50)
        trans = TransformStamped()
        while not rospy.is_shutdown():
            if(self.msgNames is None or self.msgPose is None):
                rate.sleep()
                continue

            for i in range(len(self.msgNames)):
                if(self.msgNames[i] in self.baseLinks.keys()):
                    #print(msg.name[i])
                    #print(msg.pose[i])
                    trans.header.frame_id = "world"
                    trans.child_frame_id = self.baseLinks[self.msgNames[i]]

                    trans.header.stamp = rospy.Time.now()

                    trans.transform.translation.x = self.msgPose[i].position.x
                    trans.transform.translation.y = self.msgPose[i].position.y
                    trans.transform.translation.z = self.msgPose[i].position.z

                    trans.transform.rotation.x = self.msgPose[i].orientation.x
                    trans.transform.rotation.y = self.msgPose[i].orientation.y
                    trans.transform.rotation.z = self.msgPose[i].orientation.z
                    trans.transform.rotation.w = self.msgPose[i].orientation.w

                    self.tfBroadcaster.sendTransform(trans)
            rate.sleep()
        '''
        rospy.spin()

if __name__ == '__main__':
    prefix = "measuring_system"

    if(len(sys.argv)>1):
        prefix = sys.argv[1]
        print("file written to prefix",prefix)

    server = measuring_system_service('measuring_system_service',prefix)
    server.run()
